#include <general_interface/action/switch_connection.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>

using namespace std::chrono_literals;

class ConnectionActionInvokerNode : public rclcpp::Node
{
  private:
    using SwitchaAction = general_interface::action::SwitchConnection;
    using GoalHandle = rclcpp_action::ClientGoalHandle<SwitchaAction>;
    rclcpp_action::Client<SwitchaAction>::SharedPtr action_client;
    rclcpp::TimerBase::SharedPtr timer;

  public:
    explicit ConnectionActionInvokerNode(const rclcpp::NodeOptions &options)
        : Node("connection_action_invoker", options)
    {
        action_client = rclcpp_action::create_client<SwitchaAction>(this, "connection");
        timer = create_wall_timer(100ms, std::bind(&ConnectionActionInvokerNode::timer_callback, this));
    }

  private:
    void timer_callback()
    {
        this->timer->cancel();
        if (!this->action_client->wait_for_action_server())
        {
            RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
            rclcpp::shutdown();
        }
        auto goal = SwitchaAction::Goal();
        goal.command = SwitchaAction::Goal::CONNECT;
        goal.timeout = 10;
        auto send_goal_options = rclcpp_action::Client<SwitchaAction>::SendGoalOptions();
        send_goal_options.goal_response_callback =
            std::bind(&ConnectionActionInvokerNode::on_goal_response, this, std::placeholders::_1);
        send_goal_options.feedback_callback =
            std::bind(&ConnectionActionInvokerNode::on_feedback, this, std::placeholders::_1, std::placeholders::_2);
        send_goal_options.result_callback =
            std::bind(&ConnectionActionInvokerNode::on_result, this, std::placeholders::_1);
        action_client->async_send_goal(goal, send_goal_options);
    }

    void on_goal_response(const GoalHandle::SharedPtr &handle)
    {
        if (handle != nullptr)
        {
            RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
        }
        else
        {
            RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
        }
    }
    void on_feedback(const GoalHandle::SharedPtr, const SwitchaAction::Feedback::ConstSharedPtr feedback)
    {
        switch (feedback->current)
        {
        case SwitchaAction::Result::PAUSED:
            RCLCPP_INFO(this->get_logger(), "FeedBack: Paused");
            break;
        case SwitchaAction::Result::CONNECTING:
            RCLCPP_INFO(this->get_logger(), "FeedBack: Connecting to the robot...");
            break;
        case SwitchaAction::Result::CONNECTED:
            RCLCPP_INFO(this->get_logger(), "FeedBack: Connected to the robot");
            break;
        case SwitchaAction::Result::DISCONNECTING:
            RCLCPP_INFO(this->get_logger(), "FeedBack: Disconnecting from the robot...");
            break;
        case SwitchaAction::Result::DISCONNECTED:
            RCLCPP_INFO(this->get_logger(), "FeedBack: Disconnected from the robot");
            break;
        default:
            RCLCPP_ERROR(this->get_logger(), "FeedBack: Unknown feedback state");
        }
    }
    void on_result(const GoalHandle::WrappedResult &result)
    {
        if (result.result->success)
        {
            RCLCPP_INFO(this->get_logger(), "Action finished with success");
        }
        switch (result.result->result)
        {
        case SwitchaAction::Result::PAUSED:
            RCLCPP_INFO(this->get_logger(), "Result: Paused");
            break;
        case SwitchaAction::Result::CONNECTING:
            RCLCPP_INFO(this->get_logger(), "Result: Connecting to the robot...");
            break;
        case SwitchaAction::Result::CONNECTED:
            RCLCPP_INFO(this->get_logger(), "Result: Connected to the robot");
            break;
        case SwitchaAction::Result::DISCONNECTING:
            RCLCPP_INFO(this->get_logger(), "Result: Disconnecting from the robot...");
            break;
        case SwitchaAction::Result::DISCONNECTED:
            RCLCPP_INFO(this->get_logger(), "Result: Disconnected from the robot");
            break;
        default:
            RCLCPP_ERROR(this->get_logger(), "Result: Unknown feedback state");
        }
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<ConnectionActionInvokerNode>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}